/*
 * main.c - AT91SAM7 hello world example program.
 *
 * Copyright (c) 2007 by Wolfgang Wieser ] wwieser (a) gmx <*> de [
 */


// Dit programma zou je kunnen omwerken voor de LegoNXT ,kos

#include "at91sam7s256.h"

typedef signed char int8;
typedef unsigned char uint8;

#define nop()  __asm__ __volatile__("nop")

// 1U betekent literal 1 unsigned
#define LED_A      (1U<<0)   /* PA0, pin 48 */
#define LED_B      (1U<<1)   /* PA1, pin 47 */

static void DefaultInterruptHandler(void)
    { /* Do nothing. */ }
static void SpuriousInterruptHandler(void)
    { /* Never do anything; just return as quickly as possible. */ }

// Endless loop of LED_A (PA0) blinks for error diagnosis.
extern void PanicBlinker(uint8 code);
void PanicBlinker(uint8 code)
{
    volatile AT91PS_PIO pPIO = AT91C_BASE_PIOA;
    pPIO->PIO_PER |= LED_A;  // Allow PIO to control LEDs.
    pPIO->PIO_OER |= LED_A;  // Enable output.
    pPIO->PIO_SODR = LED_A;  // Start with LED off.

    for(;;)
    {
        uint8 i;
        unsigned int j;
        for(i=0; i<code; i++)
        {
            pPIO->PIO_CODR = LED_A;  // LOW = turn LED on.
            for(j=300000; j; j--)  nop();
            pPIO->PIO_SODR = LED_A;     // HIGH = turn LED off.
            for(j=300000; j; j--)  nop();
        }

        for(j=300000*3; j; j--)  nop();  // Wait some time...
    }
}

// Hardware initialization function.
static void Initialize(void)
{
    // Set Flash Wait sate: 0 wait states.
    AT91C_BASE_MC->MC_FMR = ((AT91C_MC_FMCN)&(22 <<16)) | AT91C_MC_FWS_0FWS;

    // Disable watchdog.
    AT91C_BASE_WDTC->WDTC_WDMR= AT91C_WDTC_WDDIS;

    // Start up the main oscillator.
    AT91PS_PMC pPMC = AT91C_BASE_PMC;
    pPMC->PMC_MOR = (( AT91C_CKGR_OSCOUNT & (6U <<8)) | AT91C_CKGR_MOSCEN );
    while(!(pPMC->PMC_SR & AT91C_PMC_MOSCS));

    // Select master clock (MCK): Main oscillator.
    pPMC->PMC_MCKR = AT91C_PMC_CSS_MAIN_CLK;
    while(!(pPMC->PMC_SR & AT91C_PMC_MCKRDY));
    pPMC->PMC_MCKR = AT91C_PMC_CSS_MAIN_CLK | AT91C_PMC_PRES_CLK;
    while(!(pPMC->PMC_SR & AT91C_PMC_MCKRDY));

    // Enable user reset. This aids in debugging.
    AT91C_BASE_RSTC->RSTC_RMR = 0xa5000400U | AT91C_RSTC_URSTEN;

    // Set up the default interrupt handlers. 0 = FIQ, 1 = SYS.
    int i;
    for(i=0; i<31; i++)
    {  AT91C_BASE_AIC->AIC_SVR[i] = (unsigned)&DefaultInterruptHandler;  }
    AT91C_BASE_AIC->AIC_SPU = (unsigned)&SpuriousInterruptHandler;

    // Set up the IOs.
    volatile AT91PS_PIO pPIO = AT91C_BASE_PIOA;
    pPIO->PIO_PER = LED_A | LED_B;   // Allow PIO to control LEDs.
    pPIO->PIO_OER = LED_A | LED_B;   // Enable outputs for LED pins.
    pPIO->PIO_SODR = LED_A | LED_B;  // Set outputs HIGH to turn LEDs off.
}

int main(void)
{
    Initialize();

    // *(int*)0x800000=177;             // <-- Causes data abort.
    // (*((void(*)(void))0x800000))();  // <-- Causes prefetch abort.

    // Toggle LEDs as fast as the processor can.
    // You need an oscilloscope to see this.
    volatile AT91PS_PIO pPIO = AT91C_BASE_PIOA;
    for(;;)
    {
        // Toggle...
        pPIO->PIO_CODR = LED_A;
        pPIO->PIO_SODR = LED_A;
        // Toggle again...
        pPIO->PIO_CODR = LED_A;
        pPIO->PIO_SODR = LED_A;

        // Wait a bit...
        int i;
        for(i=0; i<10; i++) nop();
    }

    return(0);
}
